/*
 * @Author: EltonLi lijiaming@uavi-tech.com
 * @Date: 2023-09-07 18:32:02
 * @LastEditors: EltonLi lijiaming@uavi-tech.com
 * @LastEditTime: 2023-09-11 10:54:44
 * @FilePath: \zino-fc-v4\ZINO\filter\filter.h
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
/*** 
 * @Author: LVGRAPE
 * @Date: 2023-05-18 11:21:11
 * @LastEditTime: 2023-09-14 11:50:06
 * @LastEditors: LVGRAPE
 * @Description: 
 * @FilePath: \ZINO_FC_V4\ZINO\filter\filter.h
 * @可以输入预定的版权声明、个性签名、空行等
 */

#ifndef __FILTER_H_
#define __FILTER_H_


#include <stdint.h>
typedef struct 
{
    float Last_P;//上次估算协方差 不可以为0 ! ! ! ! ! 
    float Now_P;//当前估算协方差
    float out;//卡尔曼滤波器输出
    float Kg;//卡尔曼增益
    float Q;//过程噪声协方差
    float R;//观测噪声协方差
}Kalman_t;

typedef struct 
{
	float a1;
	float a2;
	float b0;
	float b1;
	float b2;
	float delay_element_1;
	float delay_element_2;
} lpf2pData;


float KalmanFilter(Kalman_t *kfp,float input);
void KalmanFilterInit(Kalman_t *kfp);
void KalmanFilterInitParam(Kalman_t *kfp, float q, float r);
void lpf2pInit(lpf2pData* lpfData, float sample_freq, float cutoff_freq);
float lpf2pApply(lpf2pData* lpfData, float sample);

#endif
